/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.techhighteam675.robot2012;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.PIDSource;

/**
 *
 * @author Noah Dove
 */
class ThetaPIDSource implements PIDSource 
{
public ThetaPIDSource(Gyro _gyro, Encoder _encoder, float _startAngle)
	{
                gyro = _gyro;
                encoder = _encoder;
		encoderOffset = _startAngle;
		setPoint = _startAngle;
	}
	
	public double pidGet() {
		return gyro.pidGet() + getEncoderValue() - setPoint;
	}
	
	public void setEncoderValue(float _encoderValue) {
		encoderOffset = (float) (_encoderValue - encoder.getDistance());
	}
	
	public double getEncoderValue() {
		return encoder.pidGet() + encoderOffset;
	}
	
	void changeSetPoint(float change) {
		setPoint += change;
		double encoderValueGoal = setPoint - gyro.pidGet(); //encoder value that would make PIDGet() return 0
		if(true) {
			if(encoderValueGoal < 0) {
				setPoint -= encoderValueGoal;
			}
			else if(encoderValueGoal > 360) {
				setPoint -= 360 - encoderValueGoal;
			}
		}
	}
private Gyro gyro; //gyro that measures the rotation of the robot
private	Encoder encoder; //encoder that measures the rotation of the turret
private	float encoderOffset; //difference between the encoder reading and the actual turret angle(degrees)
private	float setPoint; //the angle that you are turning the turret to with respect to the field
private	boolean stop; //if true: stop at the edges. if false: turn back around at the edges
};